/*
 * @Descripttion: 
 * @version: V1.0.0
 * @Author: LeonHe
 * @Date: 2021-05-23 21:38:37
 * @LastEditTime: 2021-05-30 16:39:03
 */
#include "main.h"
#include "Foc.h"
#include "svpwm.h"
#include "elcCurrent.h"
#include "arm_math.h"
#include "SEGGER_RTT.h"
#include "stmflash.h"
#include "ParamMng.h"
#include "commonfilter.h"


extern uint16_t usRegHoldBuf[];

extern uint16_t adcBuff[3];
extern int16_t gEncAngle;


int16_t setElcAngle = 0;
focDataStDef focDataSt = {0};
focCalbStDef focCalbSt = {0};


avgFilterQ15_EXPORT(speedFilter, 5)
avgLimitFilterQ31_EXPORT(posSumFilter, 12, 328*2)//32768*(3000RPM/60/5000)

void focInit(void)
{
    //读取配置信息
    paramMngInit(0);
    
    //更新编码器和电流传感器校准数据
    if(pParamListSt->basicCalibrated == 1)
    {
        focCalbSt.EncZeroOffset = pParamListSt->encZero;
        focCalbSt.CurrentOffset[0] = pParamListSt->CurrentOffsetA;
        focCalbSt.CurrentOffset[1] = pParamListSt->CurrentOffsetB;
    }
    
    //更新电流环PID参数
    focDataSt.IdPid.param.Kp = pParamListSt->focIdPidKp;
    focDataSt.IdPid.param.Ki = pParamListSt->focIdPidKi;
    focDataSt.IdPid.param.Kd = pParamListSt->focIdPidKd;
    focDataSt.IqPid.param.Kp = pParamListSt->focIqPidKp;
    focDataSt.IqPid.param.Ki = pParamListSt->focIqPidKi;
    focDataSt.IqPid.param.Kd = pParamListSt->focIqPidKd;
    arm_pid_init_q15(&focDataSt.IdPid.param, 1);
    arm_pid_init_q15(&focDataSt.IqPid.param, 1);

    //更新速度环PID参数
    focDataSt.SpdPid.param.Kp = pParamListSt->focSpdPidKp;
    focDataSt.SpdPid.param.Ki = pParamListSt->focSpdPidKi;
    focDataSt.SpdPid.param.Kd = pParamListSt->focSpdPidKd;
    arm_pid_init_q15(&focDataSt.SpdPid.param, 1);

    //更新位置环PID参数
    focDataSt.PosPid.param.Kp = pParamListSt->focPosPidKp<<0;
    focDataSt.PosPid.param.Ki = pParamListSt->focPosPidKi<<0;
    focDataSt.PosPid.param.Kd = pParamListSt->focPosPidKd<<0;
    arm_pid_init_q15(&focDataSt.PosPid.param, 1);
    
    
    //启动定时器和ADC外设
    LL_TIM_EnableCounter(TIM1);
    LL_ADC_REG_StartConversion(ADC1);
}

void focLoop(void)
{
    q31_t temp[2];

    //获取电流
    elcCurrentUpdate(&focDataSt.Ia, &focDataSt.Ib);

    //计算位置、速度、电角度
    encoderUpdate();

    //计算电角度的三角函数值
    focDataSt.sinVal = arm_sin_q31((q31_t)focDataSt.elcAngle<<16);
    focDataSt.cosVal = arm_cos_q31((q31_t)focDataSt.elcAngle<<16);

    //clarck变换
    arm_clarke_q31((q31_t)focDataSt.Ia<<16, (q31_t)focDataSt.Ib<<16, &temp[0], &temp[1]);
    focDataSt.Ialpha = temp[0]>>16;
    focDataSt.Ibeta  = temp[1]>>16;

    //park变换
    arm_park_q31(temp[0], 
                temp[1], 
                &temp[0], 
                &temp[1],
                focDataSt.sinVal,
                focDataSt.cosVal);
    focDataSt.IdPid.fbk = -(temp[0]>>16);
    focDataSt.IqPid.fbk = -(temp[1]>>16);

    //校准中不进行控制
    if(focDataSt.focCalibrating)
    {
        return;
    }

    if(pParamListSt->basicCalibrated != 1)
    {
        TIM1->CCR1 = (TIM1->ARR+1)/2-1;
        TIM1->CCR2 = (TIM1->ARR+1)/2-1;
        TIM1->CCR3 = (TIM1->ARR+1)/2-1;
        return;
    }
    #if 0

    //设置目标电角度指向超前当前电角度90°
    setElcAngle = focDataSt.elcAngle + 0x2000;
    if(setElcAngle < 0)
    {
        setElcAngle += 0x8000;
    }

    //将设置的目标电角度进行输出
    int16_t a,b;
    svpwmTheta2ab16(setElcAngle, 0x2000, &a, &b);
    svpwmCal(a, b, &focDataSt.out);
    #elif 0
    //设置电角度不断增加
    setElcAngle += 10;
    if(setElcAngle < 0 || setElcAngle > 0x8000)
    {
        setElcAngle = 0;
    }

    //计算并输出
    int16_t a,b;
    svpwmTheta2ab16(setElcAngle, 0x2000, &a, &b);
    svpwmCal(a, b, &focDataSt.out);
    #else
    memcpy(&focDataSt.IdPid.ref, &usRegHoldBuf[512+3], 2);
    memcpy(&focDataSt.IqPid.ref, &usRegHoldBuf[512+4], 2);
    memcpy(&focDataSt.SpdPid.ref, &usRegHoldBuf[512+5], 2);
    memcpy(&focDataSt.PosPid.ref, &usRegHoldBuf[512+6], 2);
    focDataSt.SpdPid.ref *= 10;
    focDataSt.PosPid.ref <<= 0;
    
    switch(pParamListSt->focMode)
    {
        case 3:
            PidControllerQ15(&focDataSt.PosPid);
            focDataSt.SpdPid.ref = (q15_t)(focDataSt.PosPid.out >> 0);
        case 2:
            // if(focDataSt.SpdPid.ref - focDataSt.SpdPid.fbk < 30 && focDataSt.SpdPid.ref - focDataSt.SpdPid.fbk > -30)
            //     focDataSt.SpdPid.fbk = focDataSt.SpdPid.ref;
            PidControllerQ15(&focDataSt.SpdPid);
            focDataSt.IqPid.ref = focDataSt.SpdPid.out;
            focDataSt.IdPid.ref = 0;
        case 1:
            PidControllerQ15(&focDataSt.IdPid);
            PidControllerQ15(&focDataSt.IqPid);
            break;
        default:
            TIM1->CCR1 = (TIM1->ARR+1)/2-1;
            TIM1->CCR2 = (TIM1->ARR+1)/2-1;
            TIM1->CCR3 = (TIM1->ARR+1)/2-1;
            return;

    }

    //反park变换
    arm_inv_park_q31(focDataSt.IdPid.out<<16,
                    focDataSt.IqPid.out<<16,
                    &temp[0], 
                    &temp[1],
                    focDataSt.sinVal,
                    focDataSt.cosVal);
    
    //svpwm计算
    svpwmCal(temp[0]>>16, temp[1]>>16, &focDataSt.out);

    #endif

    //计算占空比并设置到PWM
    for(uint8_t i = 0; i < 3; i++)
    {
        focDataSt.ccrTemp[i] = (int32_t)focDataSt.out.pwmDutyPresent[i]*(TIM1->ARR+1)/0x8000-1;
    }
    TIM1->CCR1 = focDataSt.ccrTemp[0];
    TIM1->CCR2 = focDataSt.ccrTemp[1];
    TIM1->CCR3 = focDataSt.ccrTemp[2];
}

void focCalibrate(void)
{
    int32_t sumTemp[2] = {0};
    
    focDataSt.focCalibrating = 1;
    int16_t a,b;
    svpwmTheta2ab16(0, 0x4000, &a, &b);
    svpwmCal(a, b, &focDataSt.out);

    for(uint8_t i = 0; i < 3; i++)
    {
        focDataSt.ccrTemp[i] = (int32_t)focDataSt.out.pwmDutyPresent[i]*(TIM1->ARR+1)/0x8000-1;
    }
    TIM1->CCR1 = focDataSt.ccrTemp[0];
    TIM1->CCR2 = focDataSt.ccrTemp[1];
    TIM1->CCR3 = focDataSt.ccrTemp[2];

    HAL_Delay(1500);
    sumTemp[0] = 0;
    for(uint16_t i = 0; i < 300; i++)
    {
        sumTemp[0] += gEncAngle;
        HAL_Delay(5);
    }
    focCalbSt.EncZeroOffset = sumTemp[0]/300;

    LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH1);
    LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH1N);
    LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH2);
    LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH2N);
    LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH3);
    LL_TIM_CC_DisableChannel(TIM1, LL_TIM_CHANNEL_CH3N);
    
    HAL_Delay(1500);
    sumTemp[0] = 0;
    sumTemp[1] = 0;
    for(uint16_t i = 0; i < 300; i++)
    {
        sumTemp[0] += adcBuff[0];
        sumTemp[1] += adcBuff[1];
        HAL_Delay(5);
    }
    focCalbSt.CurrentOffset[0] = sumTemp[0]/300;
    focCalbSt.CurrentOffset[1] = sumTemp[1]/300;

    LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1);
    LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH1N);
    LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2);
    LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH2N);
    LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3);
    LL_TIM_CC_EnableChannel(TIM1, LL_TIM_CHANNEL_CH3N);

    arm_pid_reset_q15(&focDataSt.IqPid.param);
    arm_pid_reset_q15(&focDataSt.IdPid.param);
    
    focDataSt.IqPid.out = 0;
    focDataSt.IqPid.fbk = 0;
    focDataSt.IdPid.out = 0;
    focDataSt.IdPid.fbk = 0;

    pParamListSt->encZero = focCalbSt.EncZeroOffset;
    pParamListSt->CurrentOffsetA = focCalbSt.CurrentOffset[0];
    pParamListSt->CurrentOffsetB = focCalbSt.CurrentOffset[1];
    pParamListSt->basicCalibrated = 1;
    paramMngWrite();

    focDataSt.focCalibrating = 0;

}

static void encoderUpdate(void)
{
    q15_t angleRaw, angle;
    q15_t angleDiff;
    q31_t position;
    static q15_t angleRawLast = 0, angleLast = 0;
    static q15_t angleRawMult = 0, angleMult = 0;

    //计算位置
    angleRaw = (gEncAngle - focCalbSt.EncZeroOffset);
    if(angleRaw > angleRawLast)
    {
       if(angleRaw - angleRawLast > 0x4000)
       {
           angleRawMult--;
       } 
    }
    else
    {
       if(angleRawLast - angleRaw > 0x4000)
       {
           angleRawMult++;
       } 
    }
    angleRawLast = angleRaw;
    position = avgLimitFilterQ31(&posSumFilter, (q31_t)angleRawMult*0x8000 + angleRaw);

    //计算角度
    angle = position%0x8000;
    if(angle < 0)
    {
        angle += 0x8000;
    }

    //计算速度
    if(angle > angleLast)
    {
       if(angle - angleLast > 0x4000)
       {
           angleDiff = (angle - 0x8000) - angleLast;
           angleMult--;
       } 
       else
       {
           angleDiff = angle - angleLast;
       }
    }
    else
    {
       if(angleLast - angle > 0x4000)
       {
           angleDiff = angle - (angleLast - 0x8000);
           angleMult++;
       } 
       else
       {
           angleDiff = angle - angleLast;
       }
    }
    angleLast = angle;
    focDataSt.SpdPid.fbk = (q15_t)((q31_t)avgFilterQ15(&speedFilter, angleDiff)*60*5000*10/0x8000);//10*RPM
    focDataSt.PosPid.fbk = (q15_t)((q31_t)angleMult*360 + (q31_t)angle*360/0x8000);

    //计算电角度
    focDataSt.elcAngle = angle*7;
    if(focDataSt.elcAngle < 0)
    {
        focDataSt.elcAngle += 0x8000;
    }
}






